package edu.mapi.ir.state;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.ActionEvent;
import java.awt.event.MouseAdapter;
import java.awt.event.MouseEvent;
import java.awt.geom.AffineTransform;
import java.awt.geom.Arc2D;
import java.awt.geom.Area;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.awt.image.BufferedImage;
import java.util.LinkedHashMap;
import java.util.Vector;
import java.util.concurrent.locks.ReentrantLock;
import java.util.prefs.Preferences;

import javax.swing.AbstractAction;
import javax.swing.JPanel;
import javax.swing.JPopupMenu;

import edu.mapi.ir.MazeWalker;
import edu.mapi.ir.controllers.AStarController;
import edu.mapi.ir.controllers.GoToController;
import edu.mapi.ir.controllers.SimpleReactiveController;
import edu.mapi.ir.controllers.StopController;
import edu.mapi.ir.gui.MazeSlammer;
import edu.mapi.ir.gui.colormaps.ColorMap;
import edu.mapi.ir.gui.colormaps.ColorMapFactory;
import edu.mapi.ir.gui.colormaps.InterpolationColorMap;
import edu.mapi.ir.planning.SlamSearch;
import edu.mapi.ir.viewer.MousePlayer;
/**
 * @author ZP
 */
public class EstimatedMaze extends JPanel {

	public static int RESOLUTION = 10;
	
	public static int MAX_WIDTH = 28 * 2;
	public static int MAX_HEIGHT = 14 * 2;
	
	private double left = 0 ;
	private double right = MAX_WIDTH;
	private double bottom = 0 ;
	private double top = MAX_HEIGHT;

	private double noise_obstacle = 0.1;
	private double min_wall_width = 0.6;
	private Vector<Point2D> plannedPath = null;
	private MazeSlammer mazeSlammer;
	private ReentrantLock lock = new ReentrantLock();
	public EstimatedCell[][] cells = new EstimatedCell[MAX_WIDTH * RESOLUTION][MAX_HEIGHT * RESOLUTION];
	
	private  BufferedImage map = new BufferedImage(MAX_WIDTH * RESOLUTION, MAX_HEIGHT * RESOLUTION, BufferedImage.TYPE_INT_ARGB);
	private EstimatedState state = null;
	private MousePlayer mouse = new MousePlayer(new Point2D.Double(MAX_WIDTH/2, MAX_HEIGHT/2));

	public static LinkedHashMap<String, ColorMap> maps = new LinkedHashMap<String, ColorMap>();
	static {
		maps.put("Red, Black, Blue", new InterpolationColorMap(new double[]{0.0, 0.90, 1.0}, new Color[]{Color.blue, Color.black, Color.red}));
		maps.put("Hot", ColorMapFactory.createHotColorMap());
		maps.put("Cool", ColorMapFactory.createCoolColorMap());
		maps.put("Autumm", ColorMapFactory.createAutummColorMap());
		maps.put("Red, Yellow, Green", new InterpolationColorMap(new double[]{0.0, 0.90, 1.0}, new Color[]{Color.green.darker().darker(), Color.yellow, Color.red}));
		maps.put("Grayscale", new InterpolationColorMap(new double[]{0.0, 0.60, 1.0}, new Color[]{Color.black, Color.black, Color.white}));		
	}
	
	Preferences prefs = Preferences.systemNodeForPackage(MazeWalker.class);
	public String colormap = prefs.get("colormap", "Red, Black, Blue");
	private ColorMap cmap = maps.get(colormap);

	
	public void setColorMap(String colormap) {
		lock.lock();
		if (maps.containsKey(colormap)) {
			prefs.put("colormap", colormap);
			this.colormap = colormap;
			this.cmap = maps.get(colormap);
			for (int x = 0; x < MAX_WIDTH * RESOLUTION; x++) {
				for (int y = 0; y < MAX_HEIGHT * RESOLUTION; y++) {
					map.setRGB(x, y, cmap.getColor(cells[x][y].getWallProbability()).getRGB());
				}
			}
		}
		lock.unlock();
		repaint();

		
	}
	
	public String getColorMap() {
		return colormap;
	}
	
	public double getMazeWidth() {
		return MAX_WIDTH;
	}

	public double getMazeHeight() {
		return MAX_HEIGHT;
	}

	
	public void clearMap() {
		for (int x = 0; x < MAX_WIDTH * RESOLUTION; x++) {
			for (int y = 0; y < MAX_HEIGHT * RESOLUTION; y++) {
				cells[x][y].setWallProbability(0.5);
				map.setRGB(x, y, cmap.getColor(cells[x][y].getWallProbability()).getRGB());
			}
		}
		repaint();
	}
	
	public void setResolution(int res) {
		lock.lock();
		EstimatedMaze.RESOLUTION = res;		
		map = new BufferedImage(MAX_WIDTH * RESOLUTION, MAX_HEIGHT * RESOLUTION, BufferedImage.TYPE_INT_ARGB);
		clearMap();
		lock.unlock();
	}
	
	public double getWallProbability(double x, double y) {
		x *= RESOLUTION;
		y *= RESOLUTION;
		
		if (x < 0 || x > MAX_WIDTH * RESOLUTION)
			return 1.0;
		
		if (y < 0 || y > MAX_HEIGHT * RESOLUTION)
			return 1.0;
		
		return cells[(int)x][(int)y].getWallProbability();
	}
	
	public void setEstimatedState(EstimatedState state) {
		this.state = state;
		
		for (int i = 0; i < 4; i++) {
			mouse.distanceSensors[i] = state.distanceSensors[i];
			mouse.setPosition(new Point2D.Double(state.getX(), state.getY()));
			mouse.setDirection(state.getRotation());
		}
		
		double startAngle[] = new double[] {-30, -90, 30, -120};
		double xoff[] = new double[4];
		double yoff[] = new double[4];
		
		for (int i = 0; i < 4; i++) {
			xoff[i] = Math.cos(startAngle[i]+30) / 2;
			yoff[i] = Math.sin(startAngle[i]+30) / 2;
		}
		
		double maxDist = 0;
		
		Area areaA = new Area();
		Area areaB = new Area();
		Area areaC = new Area();
				
		for (int i = 0; i < 3; i++) {
			double dist = state.distanceSensors[i];
			
			if (dist < 0.3)
				continue;
			
			double distA = 1/(dist + noise_obstacle);
			double distB = 1/dist;
			double distC = distB + min_wall_width;
						
			maxDist = Math.max(maxDist, distC);
			
			Arc2D.Double arcA = new Arc2D.Double(state.getX()+xoff[i]-distA, state.getY()+yoff[i]-distA, distA*2, distA*2, startAngle[i]+Math.toDegrees(-state.getRotation()), 60.0,Arc2D.PIE);			
			Arc2D.Double arcB = new Arc2D.Double(state.getX()+xoff[i]-distB, state.getY()+yoff[i]-distB, distB*2, distB*2, startAngle[i]+Math.toDegrees(-state.getRotation()), 60.0,Arc2D.PIE);
			Arc2D.Double arcC = new Arc2D.Double(state.getX()+xoff[i]-distC, state.getY()+yoff[i]-distC, distC*2, distC*2, startAngle[i]+Math.toDegrees(-state.getRotation()), 60.0,Arc2D.PIE);
			
			areaA.add(new Area(arcA));
			areaB.add(new Area(arcB));
			areaC.add(new Area(arcC));
		}
		
		lock.lock();
		
		for (int x = (int)((state.getX() - maxDist)*RESOLUTION); x < (state.getX()+maxDist) * RESOLUTION; x++) {
			for (int y = (int)((state.getY() - maxDist)*RESOLUTION); y < (state.getY()+maxDist) * RESOLUTION; y++) {
				
				if (x < 0 || y < 0 || x >= MAX_WIDTH * RESOLUTION || y >= MAX_HEIGHT * RESOLUTION)
					continue;
				
				double cx = (double)x/(double)RESOLUTION;								
				double cy = (double)y/(double)RESOLUTION;
				
				boolean paint = false;
				
				double multiplier = 1.0;
				
				if (Point2D.distance(state.getX(), state.getY(), cx, cy) < 0.5) {
					multiplier = 0.85;
					paint = true;
				}
				// area A
				else if (areaA.contains(cx, cy)) {
					multiplier = 0.95;
					paint = true;
				}
				// area B
				else if (areaB.contains(cx, cy)) {
					multiplier = 0.97;
					paint = true;
				}
				// area C
				else if (areaC.contains(cx, cy)) {									
					multiplier = 1.04;
					paint = true;
				}
								
				if (paint) {
					try {
						cells[x][y].setWallProbability(cells[x][y].getWallProbability() * multiplier);
						map.setRGB(x, y, cmap.getColor(cells[x][y].getWallProbability()).getRGB());
					}
					catch (Exception e) {
						return;
					}
				}
			}
		}
		lock.unlock();
		repaint();
	}
	
	
	public void paintLayer(Graphics2D g) {
				
		
		g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
		
		g.scale(1, -1);
		g.translate(0, -MAX_HEIGHT*RESOLUTION);
		g.drawImage(map, 0, 0, null);

	}
	
	public Point2D getWorldCoordinates(Point2D screenCoordinates) {
		double zoomW = (double)(MAX_WIDTH)/(double)getWidth();
		double zoomH = (double)(MAX_HEIGHT)/(double)getHeight();
		
		return new Point2D.Double(zoomW * screenCoordinates.getX(),
				zoomH * (getHeight()-screenCoordinates.getY()));
		
	}
	
	public EstimatedMaze(MazeSlammer mainApp) {
		
		this.mazeSlammer = mainApp;
		
		setBackground(Color.black);
		
		for (int x = 0; x < MAX_WIDTH * RESOLUTION; x++) {
			for (int y = 0; y < MAX_HEIGHT * RESOLUTION; y++) {
				cells[x][y] = new EstimatedCell((double)x/(double)RESOLUTION,(double)y/(double)RESOLUTION);
				map.setRGB(x, y, cmap.getColor(cells[x][y].getWallProbability()).getRGB());
			}
		}
		
		MouseAdapter myAdapter = new MouseAdapter() {
			public void mouseClicked(MouseEvent e) {
				super.mouseClicked(e);
				
				if (e.getButton() != MouseEvent.BUTTON3)
					return;
				
				JPopupMenu popup = new JPopupMenu();
				
				Point2D pt = getWorldCoordinates(e.getPoint());
				final int x = (int)pt.getX();
				final int y = (int)pt.getY();
				
				AbstractAction astar = new AbstractAction("A* to ("+x+","+y+")") {
					public void actionPerformed(ActionEvent e) {
						plannedPath = new SlamSearch(EstimatedMaze.this).getBestPath(new Point2D.Double(state.getX(), state.getY()), new Point2D.Double(x,y), 0);
						if (mazeSlammer != null && mazeSlammer.getClient() != null)
							mazeSlammer.getClient().setActiveController(new AStarController(plannedPath));
						repaint();
					}
				};
				popup.add(astar);
				
				AbstractAction stop = new AbstractAction("Stop!") {
					public void actionPerformed(ActionEvent e) {
						if (mazeSlammer != null && mazeSlammer.getClient() != null)
							mazeSlammer.getClient().setActiveController(new StopController());
						plannedPath = null;
						repaint();
					}
				};
				popup.add(stop);
				
				AbstractAction gotoxy = new AbstractAction("Goto ("+x+","+y+")") {
					public void actionPerformed(ActionEvent e) {
						if (mazeSlammer != null && mazeSlammer.getClient() != null)
							mazeSlammer.getClient().setActiveController(new GoToController(state.getX(), state.getY(), x,y));
						plannedPath = null;
						repaint();
					}
				};
				popup.add(gotoxy);
				
				AbstractAction reactive = new AbstractAction("Wander") {
					public void actionPerformed(ActionEvent e) {
						if (mazeSlammer != null && mazeSlammer.getClient() != null)
							mazeSlammer.getClient().setActiveController(new SimpleReactiveController());
						plannedPath = null;
						repaint();
					}
				};
				popup.add(reactive);
				
				popup.show(EstimatedMaze.this, e.getX(), e.getY());
			}
		};
		
		addMouseListener(myAdapter);
	}
	
	@Override
	public void paint(Graphics arg0) {
		super.paint(arg0);
		
		Graphics2D g = (Graphics2D) arg0;
		AffineTransform t = g.getTransform();
		
		
		
		double zoomW = (double)getWidth()/(double)(MAX_WIDTH*RESOLUTION);
		double zoomH = (double)getHeight()/(double)(MAX_HEIGHT*RESOLUTION);
		
		g.scale(zoomW, zoomH);
		
		paintLayer(g);
		g.setTransform(t);
		
		g.setColor(Color.gray.darker());
		
		for (double i = 0; i < getWidth(); i+= zoomW*RESOLUTION) {
			g.draw(new Line2D.Double(i, 0, i, getHeight()));
		}
		for (double i = 0; i < getHeight(); i+= zoomH*RESOLUTION) {
			g.draw(new Line2D.Double(0, i, getWidth(), i));
		}
		
		
		g.scale(1, -1);
		g.translate(0, -getHeight());
		
		g.translate(state.getX()*zoomW*RESOLUTION, state.getY()*zoomH*RESOLUTION);
		
		
		
		if (state != null) {
			g.setColor(Color.red);
			g.rotate(Math.PI/2+mouse.getDirection());
			mouse.paintMouse(g, zoomH*RESOLUTION);			
		}
		else {
			System.out.println("state is null");
		}
		
		g.setTransform(t);
		
		if (plannedPath != null) {
			
			g.setColor(Color.green);
			double zW = (double)getWidth()/(double)MAX_WIDTH;
			double zH = (double)getHeight()/(double)MAX_HEIGHT;
			
			
			
			Point2D lastPt = plannedPath.get(0);
			for (int i = 1; i < plannedPath.size(); i++) {
				Point2D pt = plannedPath.get(i);
				g.draw(new Line2D.Double(lastPt.getX()*zW, getHeight()-lastPt.getY()* zH, pt.getX()*zW, getHeight()-pt.getY()*zH));
				lastPt = pt;
				//System.out.println(pt.getY()+"->"+pt.getY()*zH);
			}
		}
		
		if (mazeSlammer.getClient() != null && mazeSlammer.getClient().getActiveController() != null) {
			g.setColor(Color.green);
			g.drawString("Current Controller: "+mazeSlammer.getClient().getActiveController().getName(), 10, getHeight()-10);
		}
		else {
			g.setColor(Color.green);
			g.drawString("Not connected", 10, getHeight()-10);
		}
	}
	
	public boolean isObstacle(double x, double y) {
		return cells[(int)(RESOLUTION * x)][(int)(RESOLUTION * y)].getWallProbability() > 0.55; 
	}
	
	public double getLeft() {
		return left;
	}

	public void setLeft(double left) {
		this.left = left;
	}

	public double getRight() {
		return right;
	}

	public void setRight(double right) {
		this.right = right;
	}

	public double getBottom() {
		return bottom;
	}

	public void setBottom(double bottom) {
		this.bottom = bottom;
	}

	public double getTop() {
		return top;
	}

	public void setTop(double top) {
		this.top = top;
	}
}
